% aniplot(data)
%
%  Description:
%   Generates an animated plot showing the motion of the system with
%   additional plots showing the phase portrait and angle and angle
%   velocities measured over time.
%
%  Operational Description:
%   Plots the angle/angle velocity and phase portrait graphs.  Creates
%   markers as plot objects that match up with animation graph, updating
%   every time the animation graph is updated.  The animation is a plot
%   object containing points for the end of each link.  This is updated for
%   every data point to generate an animation.
%
%  Arguments:
%   data    - A struct containing a an 'integral_curves' member.  This
%             member holds the results of integration returned by
%             hybrid_flow().
%
%  Return Values:
%   None.
%
function aniplot(data, varargin)
%% Fit plots to screen.
%
screenSize = get(0, 'ScreenSize');
width = screenSize(3);
height = screenSize(4);

% Set default options
%
opts.makeavi = false;
opts.extragraphs = false;
opts.plotmarkers = false;
opts.moviefile = 'biped_walk.avi';
opts.framerate = 60;
opts.res = 0;

% Parse varargin
for k = 1:size(varargin, 2)/2
    switch lower(varargin{2*k - 1})
        case 'movie'
            if strcmpi(varargin{2*k}, 'avi')
                opts.makeavi = true;
            end

        case 'moviefile'
            opts.moviefile = varargin{2*k};
            
        case 'extragraphs'
                opts.extragraphs = varargin{2*k};
            
        case 'plotmarkers'
            opts.plotmarkers = varargin{2*k};
            
        case 'framerate'
            if ~(varargin{2*k} - floor(varargin{2*k})) && ...
                    isscalar(varargin{2*k}) %#ok<BDLOG>
                opts.framerate = varargin{2*k};
            end
            
            % set the spec to 480p or 720p or 1080p
        case 'resspec'
            if strcmpi(varargin{2*k}, '720p')
                opts.res = 1;
            elseif strcmpi(varargin{2*k}, '1080p')
                opts.res = 2;
            elseif strcmpi(varargin{2*k}, '480p')
                opts.res = 3;
            end
            
        otherwise
            error('No such option.');
    end
end

if ~opts.extragraphs
    opts.plotmarkers = false;
    opts.anglemarkers = false;
end



% Parse input.
%
curves = cat(2, data.integral_curves{:});
t = cat(2, data.time{:});
q = curves;
dim = size(q, 1);

posv = (1:floor(dim/2));
vel = (floor(dim/2)+1:dim);

%% Setup the legends.
%
names = cell(1, dim);

% domainnames = {'\mathit{sf}','\mathit{sc}','\mathit{st}', ...
%     '\mathit{nst}','\mathit{nsc}','\mathit{nsf}'};

% for n=1:length(posv)
%     names{posv(n)} = ['$\theta_' domainnames{n} '$'];
%     names{vel(n)} = ['$\dot{\theta}_' domainnames{n} '$'];
% end

for n=1:length(posv)
    names{posv(n)} = ['$q_' num2str(n) '$'];
    names{vel(n)} = ['${\dot q}_' num2str(n) '$'];
end


%% Plot the phase portraits.
%
fig = figure(1); clf; hold off;
if opts.makeavi
    set(fig,'DoubleBuffer','on');
end

% Check if the selected recording standard is possible given the
% configuration of the user's system.
%
if opts.res == 2 && width < 1920 && height < 1080
    opts.res = 1;
    disp('Warning::Configuration does not allow 1080p, switching to 720p');
end
if opts.res == 1 && width < 1280 && height < 720
    opts.res = 3;
    disp('Warning::Configuration does not allow 720p, switching to 420p');
end

% Set the screen size for the selected standard or the next best available
% standard.
%
switch opts.res
    case 1
        set(fig, 'Position', [50 50 1280 720]);
    case 2
        set(fig, 'Position', [0 0 1920 1280]);
    case 3
        set(fig, 'Position', [50 250 720 480]);
    otherwise
        set(fig, 'Position', [0 35 width height-70]);
end

linespecs = {'k.', 'k.', 'k.', 'k.', 'k.', 'k.', 'k.'};

if opts.extragraphs
    subplot(221);
    plot(q(posv, :)', q(vel, :)');
    legend(strcat(names(posv)), 'Interpreter', 'LaTeX', ...
        'Location', 'BestOutside', 'Orientation', 'Horizontal')
    legend boxoff

    hold on;
    th = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        th(i) = plot(nan, linespecs{i}, 'MarkerSize', 12);
    end

end
%% Plot the angles over time.
%
if opts.extragraphs
    subplot(422); plot(t, q(posv, :))
    legend(strcat(names(posv)), 'Interpreter', 'LaTeX', ...
        'Location', 'BestOutside', 'Orientation', 'Horizontal')
    legend boxoff
    
    hold on;
    tha = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        tha(i) = plot(nan, linespecs{i}, 'MarkerSize', 12);
    end
    
    subplot(424); plot(t, q(vel, :))
    legend(strcat(names(vel)), 'Interpreter', 'LaTeX', ...
        'Location', 'BestOutside', 'Orientation', 'Horizontal')
    legend boxoff
    
    hold on;
    thb = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        thb(i) = plot(nan, linespecs{i}, 'MarkerSize', 12);
    end
end

%% Setup the plot for the animation.
%
if opts.extragraphs
    subplot(212);
end
hold on;
axis([-12 2 -2 2]);
axis manual;
origin = [0; 0];

grid on
 legs = plot(nan);
% 
% pa = get(gca, 'Position');
% ha = axes()
% set(ha, ...
%     'Position', pa, ...
%     'Visible', 'on');
% 
xbnd = [-1 5];
dt = 1e-2;

fh = @get_terrain;

x = xbnd(1):dt:xbnd(2);
y_int = fh(0);
y = fh(x);

patch(...
    [-.5, x, 3.25], ... x
    [-.1, (y-y_int), -.1], ...
    ... %'Color', [108 52 28]/255);%, ... y
    0, ... color data
    'EdgeColor', 'None', ... no lines
    'FaceColor', [108 52 28]/255); % brown


%% Setup plot objects for the angle graphs.
%
if opts.extragraphs && opts.plotmarkers && opts.makeavi
    subplot(221); hold on;
    th = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        th(i) = plot(nan, linespecs{i}, ...
            'MarkerSize', 12);
    end
    
    subplot(422); hold on;
    tha = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        tha(i) = plot(nan, linespecs{i}, ...
            'MarkerSize', 12);
    end
    
    subplot(424); hold on;
    thb = NaN(floor(dim/2), 1);
    for i = 1:floor(dim/2)
        thb(i) = plot(nan, linespecs{i}, ...
            'MarkerSize', 12);
    end
end

if opts.makeavi
    if opts.extragraphs
        subplot(212);
    end
    set(gca, ...
        'NextPlot', 'Replace', ...
        'Visible', 'Off')
    mov = avifile(...
        opts.moviefile, ...
        'Compression', 'None', ...
        'fps', opts.framerate);
    nmod = 1;
    if ~opts.extragraphs
        set(gca, ...
            'Position', [0.12 0.22 .87 .76]);
    end
end

k = 0;
tl = 0;
tic

%% Precompute plot positions.
%
xpos = cell(length(data.integral_curves),1);
for i=1:length(data.integral_curves)
    %% Find the current positions of all the ends of the links, i.e., feet,
    %  knees, hip.
    %
    xpos{i} = jointpos(data.integral_curves{i});
end

tic

for i=1:length(data.integral_curves)
    pos = xpos{i};
    %% Loop through all the data points in each discrete state.
    %
    for j = 1:size(pos, 3);
        % fix this for making videos by adding an option
        if (data.time{i}(j) < toc) && ~opts.makeavi
           continue
        end
        
        k = k + 1;
        
        if opts.extragraphs
            subplot(212);
        end
        
        if k == 1
            axis equal
            ax = gca();
            % xl = get(ax, 'XLim');
            % yl = get(ax, 'YLim');
            
            set(ax, 'XLim', [-.5 3.25]);
            set(ax, 'YLim', [-0.1, .7]);
            
            % if opts.extragraphs
            %     set(gca, 'Position', [0.05 0.05 .90 .1]);
            % end
            % set(get(gca, 'XLabel'), ...
            %     'Position', [.51 -.3 1.00]);
        end
        
        if opts.makeavi
            if data.time{i}(j) < tl + 1/opts.framerate;
                continue;
            else
                tl = tl + 1/opts.framerate;
            end
        end
        
        % Update the positions of the legs and feet.
        %

        x_l = origin(1) + pos(1,  :, j);
        y_l = origin(2) + pos(2,  :, j);
        
        %% Replot the legs.
       
        set(legs, 'XData', x_l, 'YData', y_l, 'EraseMode', 'normal');

        %% Replot the tracking markers.
        if opts.extragraphs && opts.plotmarkers && opts.makeavi
            for ell = 1:floor(dim/2)
                set(th(ell), ...
                    'XData', data.integral_curves{i}(ell, j), ...
                    'YData', data.integral_curves{i}(ell + ...
                    floor(dim/2), j), ...
                    'EraseMode', 'normal');
                set(tha(ell), ...
                    'XData', data.time{i}(j), ...
                    'YData', data.integral_curves{i}(ell, j), ...
                    'EraseMode', 'normal');
                set(thb(ell), ...
                    'XData', data.time{i}(j), ...
                    'YData', data.integral_curves{i}(ell + ...
                    floor(dim/2), j), ...
                    'EraseMode', 'normal');
            end
        end
        
        drawnow;
        
        if opts.makeavi
               if mod(k, nmod) == 0
                F = getframe(gcf);
                mov = addframe(mov,F);
               end
        else
            %pause(.01);
        end
    end
    
    % Update the position of the stance foot
    origin = origin + pos(:, end, end);
end

if opts.makeavi
    mov = close(mov);
    close(figure(1));
end

end